#include "driverlib.h"
#include "device.h"
#include "common.h"

static unsigned int gSecCounter0 = 0;
static unsigned int gSecCounter1 = 0;
static unsigned int gSecCounter2 = 0;

static unsigned int gIntNumCounter0 = 0;
static unsigned int gIntNumCounter1 = 0;
static unsigned int gIntNumCounter2 = 0;

__interrupt void cpuTimer0ISR(void)
{
    static int cnt0 = 0;

    ++gIntNumCounter0;
    if (++cnt0 == 100)
    {
        gSecCounter0++;
        cnt0 = 0;
    }

    CPUTimer_clearInterrupt(CPUTIMER0_BASE);
}

__interrupt void cpuTimer1ISR(void)
{
    static int cnt1 = 0;

    ++gIntNumCounter1;
    if (++cnt1 == 200)
    {
        gSecCounter1++;
        cnt1 = 0;
    }

    CPUTimer_clearInterrupt(CPUTIMER1_BASE);
}

__interrupt void cpuTimer2ISR(void)
{
    static int cnt2 = 0;

    ++gIntNumCounter2;
    if (++cnt2 == 1000)
    {
        gSecCounter2++;
        cnt2 = 0;
    }

    CPUTimer_clearInterrupt(CPUTIMER2_BASE);
}

static void config_start_all_timers()
{
    Interrupt_register(INT_TIMER0, cpuTimer0ISR);
    Interrupt_register(INT_TIMER1, cpuTimer1ISR);
    Interrupt_register(INT_TIMER2, cpuTimer2ISR);

    SysCtl_enablePeripheral(SYSCTL_PERIPH_CLK_CPU0_TIMER0);
    SysCtl_enablePeripheral(SYSCTL_PERIPH_CLK_CPU0_TIMER1);
    SysCtl_enablePeripheral(SYSCTL_PERIPH_CLK_CPU0_TIMER2);

    CPUTimer_setPreScaler(CPUTIMER0_BASE, 0);
    CPUTimer_setPeriod(CPUTIMER0_BASE, 10000);
    CPUTimer_enableInterrupt(CPUTIMER0_BASE);

    CPUTimer_setPreScaler(CPUTIMER1_BASE, 0);
    CPUTimer_setPeriod(CPUTIMER1_BASE, 5000);
    CPUTimer_enableInterrupt(CPUTIMER1_BASE);

    CPUTimer_setPreScaler(CPUTIMER2_BASE, 0);
    CPUTimer_setPeriod(CPUTIMER2_BASE, 1000);
    CPUTimer_enableInterrupt(CPUTIMER2_BASE);

    Interrupt_enable(INT_TIMER0);
    Interrupt_enable(INT_TIMER1);
    Interrupt_enable(INT_TIMER2);

    CPUTimer_startTimer(CPUTIMER0_BASE);
    CPUTimer_startTimer(CPUTIMER1_BASE);
    CPUTimer_startTimer(CPUTIMER2_BASE);
}


#define RPT 2

#include <math.h>
#include <stdlib.h>

#define PI 3.141592653589793
#define SOLAR_MASS (4 * PI * PI)
#define DAYS_PER_YEAR 365.24

#define VERIFY_DOUBLE_EPS 1.0e-7

#define double_eq_beebs(exp, actual) (fabs(exp - actual) < VERIFY_DOUBLE_EPS)
#define double_neq_beebs(exp, actual) !double_eq_beebs(exp, actual)

struct body
{
  double x[3], fill, v[3], mass;
};

static struct body solar_bodies[] = {
    /* sun */
    {
        .x = {0., 0., 0.},
        .v = {0., 0., 0.},
        .mass = SOLAR_MASS},
    /* jupiter */
    {
        .x = {4.84143144246472090e+00,
              -1.16032004402742839e+00,
              -1.03622044471123109e-01},
        .v = {1.66007664274403694e-03 * DAYS_PER_YEAR,
              7.69901118419740425e-03 * DAYS_PER_YEAR,
              -6.90460016972063023e-05 * DAYS_PER_YEAR},
        .mass = 9.54791938424326609e-04 * SOLAR_MASS},
    /* saturn */
    {
        .x = {8.34336671824457987e+00,
              4.12479856412430479e+00,
              -4.03523417114321381e-01},
        .v = {-2.76742510726862411e-03 * DAYS_PER_YEAR,
              4.99852801234917238e-03 * DAYS_PER_YEAR,
              2.30417297573763929e-05 * DAYS_PER_YEAR},
        .mass = 2.85885980666130812e-04 * SOLAR_MASS},
    /* uranus */
    {
        .x = {1.28943695621391310e+01,
              -1.51111514016986312e+01,
              -2.23307578892655734e-01},
        .v = {2.96460137564761618e-03 * DAYS_PER_YEAR,
              2.37847173959480950e-03 * DAYS_PER_YEAR,
              -2.96589568540237556e-05 * DAYS_PER_YEAR},
        .mass = 4.36624404335156298e-05 * SOLAR_MASS},
    /* neptune */
    {
        .x = {1.53796971148509165e+01,
              -2.59193146099879641e+01,
              1.79258772950371181e-01},
        .v = {2.68067772490389322e-03 * DAYS_PER_YEAR,
              1.62824170038242295e-03 * DAYS_PER_YEAR,
              -9.51592254519715870e-05 * DAYS_PER_YEAR},
        .mass = 5.15138902046611451e-05 * SOLAR_MASS}};

static const int BODIES_SIZE =
    sizeof(solar_bodies) / sizeof(solar_bodies[0]);

void offset_momentum(struct body *bodies, unsigned int nbodies)
{
  unsigned int i, k;
  for (i = 0; i < nbodies; ++i)
    for (k = 0; k < 3; ++k)
      bodies[0].v[k] -= bodies[i].v[k] * bodies[i].mass / SOLAR_MASS;
}

double
bodies_energy(struct body *bodies, unsigned int nbodies)
{
  double dx[3], distance, e = 0.0;
  unsigned int i, j, k;

  for (i = 0; i < nbodies; ++i)
  {
    e += bodies[i].mass * (bodies[i].v[0] * bodies[i].v[0] + bodies[i].v[1] * bodies[i].v[1] + bodies[i].v[2] * bodies[i].v[2]) / 2.;

    for (j = i + 1; j < nbodies; ++j)
    {
      for (k = 0; k < 3; ++k)
        dx[k] = bodies[i].x[k] - bodies[j].x[k];

      distance = sqrt(dx[0] * dx[0] + dx[1] * dx[1] + dx[2] * dx[2]);
      e -= (bodies[i].mass * bodies[j].mass) / distance;
    }
  }
  return e;
}

/* ------------------------------ main ------------------------------ */

int main()
{

    SysCtl_disableWatchdog();
    Device_init();
    Interrupt_initModule();
    Interrupt_initVectorTable();
    config_start_all_timers();

  volatile int correct, res, j, tot;
  double tot_e = 0.0;

  for (j = 0; j < RPT; j++)
  {
    int i;
    offset_momentum(solar_bodies, BODIES_SIZE);
    /*printf("%.9f\n", bodies_energy(solar_bodies, BODIES_SIZE)); */
    tot_e = 0.0;
    for (i = 0; i < 100; ++i)
      tot_e += bodies_energy(solar_bodies, BODIES_SIZE);
    /*printf("%.9f\n", bodies_energy(solar_bodies, BODIES_SIZE)); */
  }
  /* Result is known good value for total energy. */
  tot = double_eq_beebs(tot_e, -16.907516382852478);

  int m, n;

  static struct body expected[] = {
      {.x = {0, 0, 0},
       .v =
           {-0.000387663407198742665776131088862,
            -0.0032753590371765706722173572274,
            2.39357340800030020670947916717e-05},
       .mass = 39.4784176043574319692197605036},
      {.x =
           {4.84143144246472090230781759601, -1.16032004402742838777840006514,
            -0.103622044471123109232735259866},
       .v =
           {0.606326392995832019749968821998, 2.81198684491626016423992950877,
            -0.0252183616598876288172892401462},
       .mass = 0.0376936748703894930478952574049},
      {.x =
           {8.34336671824457987156620220048, 4.1247985641243047894022311084,
            -0.403523417114321381049535375496},
       .v =
           {-1.01077434617879236000703713216, 1.82566237123041186229954746523,
            0.00841576137658415351916474378413},
       .mass = 0.0112863261319687668143840753032},
      {.x =
           {12.8943695621391309913406075793, -15.1111514016986312469725817209,
            -0.223307578892655733682204299839},
       .v =
           {1.08279100644153536414648897335, 0.868713018169608219842814378353,
            -0.0108326374013636358983880825235},
       .mass = 0.0017237240570597111687795033319},
      {.x =
           {15.3796971148509165061568637611, -25.9193146099879641042207367718,
            0.179258772950371181309492385481},
       .v =
           {0.979090732243897976516677772452, 0.594698998647676169149178804219,
            -0.0347559555040781037460462243871},
       .mass = 0.00203368686992463042206846779436}};

  /* Check we have the correct total energy and we have set up the
     solar_bodies array correctly. */

  res = 1;

  if (tot)
    for (m = 0; m < BODIES_SIZE; m++)
    {
      for (n = 0; n < 3; n++)
      {
        if (double_neq_beebs(solar_bodies[m].x[n], expected[m].x[n]))
        {
          res = 0;
          break;
        }

        if (double_neq_beebs(solar_bodies[m].v[n], expected[m].v[n]))
        {
          res = 0;
          break;
        }
      }
      if (double_neq_beebs(solar_bodies[m].mass, expected[m].mass))
      {
        res = 0;
        break;
      }
    }
  else
    res = 0;

  correct = res == 1 ? 1 : 0;

  StdOutInit(&ScibRegs, 921600, 12, GPIO_12_SCIB_TX);

  if (correct == 1) {
      puts("pass\n");
  } else {
      puts("fail\n");
  }

  EndSimulation();

  return 0;
}
